﻿using System;
using System.Collections.Generic;
namespace Steer
{
    public class SteeredVehicle:Vehicle
    {
        private double _maxForce = 1;
        private Vector2D _steeringForce;
        private double _arrivalThreshold = 100;

        private double _wanderAngle = 0;
        private double _wanderDistance = 10;
        private double _wanderRadius = 5;
        private double _wanderRange = 1;

        private double _avoidDistance = 300;
        private double _avoidBuffer = 20;

        private int _pathIndex = 0;
        private double _pathThreshold = 20;

        private double _inSightDist = 200;
        private double _tooCloseDist = 60;

        public SteeredVehicle()
        {
            _steeringForce = new Vector2D(0, 0);            
        }

        public double maxForce
        {
            get
            {
                return _maxForce;
            }
            set
            {
                _maxForce = value;
            }
        }


        public double arriveThreshold {
            get { return _arrivalThreshold; }
            set { _arrivalThreshold = value; }
        }

        public double wanderDistance {
            get { return _wanderDistance; }
            set { _wanderDistance = value; }
        }

        public double wanderRadius {
            get { return _wanderRadius; }
            set { _wanderRadius = value; }
        }

        public double wangderRange {
            get { return _wanderRange; }
            set { _wanderRange = value; }
        }

        public int pathIndex {
            get { return _pathIndex; }
            set { _pathIndex = value; }
        }

        public double pathThreshold {
            get { return _pathThreshold; }
            set { _pathThreshold = value; }
        }

        public double inSightDist {
            get { return _inSightDist; }
            set { _inSightDist = value; }
        }

        public double tooCloseDist {
            get { return _tooCloseDist; }
            set { _tooCloseDist = value; }
        }

        public override void update()
        {
            _steeringForce.truncate(_maxForce);
            _steeringForce = _steeringForce.divide(_mass);
            _velocity = _velocity.add(_steeringForce);
            _steeringForce = new Vector2D(0, 0);
            base.update();
        }

        public void seek(Vector2D target)
        {
            Vector2D desiredVelocity = target.subtract(_postion);
            desiredVelocity.normalize();
            desiredVelocity = desiredVelocity.multiply(_maxSpeed);
            Vector2D force = desiredVelocity.subtract(_velocity);
            _steeringForce = _steeringForce.add(force);
        }

        public void flee(Vector2D target) {
            Vector2D desiredVelocity = target.subtract(_postion);
            desiredVelocity.normalize();
            desiredVelocity = desiredVelocity.multiply(_maxSpeed);
            Vector2D force = desiredVelocity.subtract(_velocity);
            _steeringForce = _steeringForce.subtract(force);
        }

        public void arrive(Vector2D target) {
            Vector2D desiredVelocity = target.subtract(_postion);
            desiredVelocity.normalize();

            double dist = _postion.dist(target);
            if (dist>_arrivalThreshold)
            {
                desiredVelocity = desiredVelocity.multiply(_maxSpeed);
            }
            else
            {
                desiredVelocity = desiredVelocity.multiply(_maxSpeed * dist / _arrivalThreshold);
            }
            Vector2D force = desiredVelocity.subtract(_velocity);
            _steeringForce = _steeringForce.add(force);
        }

        public void pursue(Vehicle target) {
            double lookAheadTime = position.dist(target.position) / _maxSpeed;
            Vector2D predictedTarget = target.position.add(target.velocity.multiply(lookAheadTime));
            seek(predictedTarget);
        }

        public void evade(Vehicle target)
        {
            double lookAheadTime = position.dist(target.position) / _maxSpeed;
            Vector2D predictedTarget = target.position.add(target.velocity.multiply(lookAheadTime));
            flee(predictedTarget);
        }

        public void wander() {
            Vector2D center = velocity.clone().normalize().multiply(_wanderDistance);
            Vector2D offset = new Vector2D(0, 0);
            offset.length = _wanderRadius;
            offset.angle = _wanderAngle;
            Random randObj = new Random();
            _wanderAngle += randObj.NextDouble() * _wanderRange - _wanderRange * 0.5;
            Vector2D force = center.add(offset);
            _steeringForce = _steeringForce.add(force);
        }

        public void avoid(List<Circle> circles) {
            foreach (var circle in circles)
            {
                Vector2D heading = _velocity.clone().normalize();

                //障碍物和机车的位移向量
                Vector2D difference = circle.position.subtract(_postion);
                double dorProd = difference.dotProd(heading);

                //如果障碍物在机车前方
                if (dorProd>0)
                {
                    //机车的触角
                    Vector2D feeler = heading.multiply(_avoidDistance);
                    //位移在触角上的映射
                    Vector2D projection = heading.multiply(dorProd);
                    //障碍物离触角的距离
                    double dist = projection.subtract(difference).length;

                    //如果触角在计算上缓冲后和障碍物相交并且位移的映射的长度小于触角的长度，我们就说是将要发生碰撞，需要改变运行方向
                    if (dist<circle.Radius+_avoidBuffer&&projection.length<feeler.length)
                    {
                        //计算出一个90度的转向力
                        Vector2D force = heading.multiply(_maxSpeed);
                        force.angle += difference.sign(_velocity) * Math.PI / 2;

                        //通过距离障碍物的距离，调整力度大小，使之足够小但又能避开
                        force = force.multiply(1.0 - projection.length / feeler.length);

                        // 叠加与转向力上
                        _steeringForce = _steeringForce.add(force);

                        //刹车，转弯时要放慢机车的速度，里障碍物越近刹车力越大
                        _velocity = _velocity.multiply(projection.length / feeler.length);
                    }
                    
                }
            }
        }

        public void followPath(List<Vector2D> path,bool loop) {
            if (path.Count==0)
            {
                return;
            }
            Vector2D wayPoint = path[_pathIndex];
            if (wayPoint==null)
            {
                return;
            }
            if (_postion.dist(wayPoint)<_pathThreshold)
            {
                if (_pathIndex>=path.Count-1)
                {
                    if (loop)
                    {
                        _pathIndex = 0;
                    }

                }
                else
                {
                    _pathIndex++;
                }
            }
            if (_pathIndex>=path.Count-1&&!loop)
            {
                arrive(wayPoint);
            }
            else
            {
                seek(wayPoint);
            }
        }

        public void flock(List<Vehicle> vehicles) {
            Vector2D averageVelocity = _velocity.clone();
            Vector2D averagePosition = new Vector2D(0, 0);
            int inSightCount = 0;
            for (int i = 0; i < vehicles.Count; i++)
            {
                Vehicle vehicle = vehicles[i];
                if (vehicle!=this&&inSight(vehicle))
                {
                    averageVelocity = averageVelocity.add(vehicle.velocity);
                    averagePosition = averagePosition.add(vehicle.position);
                    if (tooClose(vehicle))
                    {
                        flee(vehicle.position);
                        inSightCount++;
                    }
                }
            }
            if (inSightCount>0)
            {
                averageVelocity = averageVelocity.divide(inSightCount);
                averagePosition = averagePosition.divide(inSightCount);
                seek(averagePosition);
                _steeringForce.add(averageVelocity.subtract(_velocity));
            }
        }

        private bool tooClose(Vehicle vehicle)
        {
            return _postion.dist(vehicle.position) < _tooCloseDist;
        }

        private bool inSight(Vehicle vehicle)
        {
            if (_postion.dist(vehicle.position)>_inSightDist)
            {
                return false;
            }
            Vector2D heading = _velocity.clone().normalize();
            Vector2D difference = vehicle.position.subtract(_postion);
            double dotProd = difference.dotProd(heading);
            if (dotProd<0)
            {
                return false;
            }
            return true;
        }
    }
}
